//
// Created by 26090 on 25-4-9.
//

#include "dm_imu.h"

#include <user_lib.h>

#include "bsp_dwt.h"


static uint8_t dm_imu_idx = 0;
static DM_IMU_Instance *imu_instance[DM_IMU_MAX] = {NULL};

static int float_to_uint(float x_float, float x_min, float x_max, int bits);
static float uint_to_float(int x_int, float x_min, float x_max, int bits);

static void DecodeDMIMU(CANInstance *_instance)
{
    uint8_t *rxbuff = _instance->rx_buff;
    DM_IMU_Instance *imu = (DM_IMU_Instance *)_instance->id;
    DM_IMU_Measure_s *measure = &imu->measure;

    DaemonReload(imu->daemon);
    imu->dt = DWT_GetDeltaT(&imu->feed_cnt);

    switch(rxbuff[0])
    {
    case 1:
        measure->acc_ecd[0]=rxbuff[3]<<8|rxbuff[2];
        measure->acc_ecd[1]=rxbuff[5]<<8|rxbuff[4];
        measure->acc_ecd[2]=rxbuff[7]<<8|rxbuff[6];
        measure->accel[0]=uint_to_float(measure->acc_ecd[0],ACCEL_CAN_MIN,ACCEL_CAN_MAX,16);
        measure->accel[1]=uint_to_float(measure->acc_ecd[1],ACCEL_CAN_MIN,ACCEL_CAN_MAX,16);
        measure->accel[2]=uint_to_float(measure->acc_ecd[2],ACCEL_CAN_MIN,ACCEL_CAN_MAX,16);
        break;
    case 2:
        measure->gyro_ecd[0]=rxbuff[3]<<8|rxbuff[2];
        measure->gyro_ecd[1]=rxbuff[5]<<8|rxbuff[4];
        measure->gyro_ecd[2]=rxbuff[7]<<8|rxbuff[6];
        measure->gyro[0]=uint_to_float(measure->acc_ecd[0],GYRO_CAN_MIN,GYRO_CAN_MAX,16);
        measure->gyro[1]=uint_to_float(measure->acc_ecd[1],GYRO_CAN_MIN,GYRO_CAN_MAX,16);
        measure->gyro[2]=uint_to_float(measure->acc_ecd[2],GYRO_CAN_MIN,GYRO_CAN_MAX,16);
        break;
    case 3:
        measure->angle_ecd[0]=rxbuff[3]<<8|rxbuff[2];
        measure->angle_ecd[1]=rxbuff[5]<<8|rxbuff[4];
        measure->angle_ecd[2]=rxbuff[7]<<8|rxbuff[6];

        if(measure->first_data==0)
        {
            measure->first_data=1;
            measure->yaw_offset=uint_to_float(measure->angle_ecd[1],YAW_CAN_MIN,YAW_CAN_MAX,16);
        }
        measure->pitch=uint_to_float(measure->angle_ecd[0],PITCH_CAN_MIN,PITCH_CAN_MAX,16);
        measure->yaw=loop_float_constrain((uint_to_float(measure->angle_ecd[1],YAW_CAN_MIN,YAW_CAN_MAX,16)-measure->yaw_offset),-180.0f,180.0f);
        measure->roll=uint_to_float(measure->angle_ecd[2],ROLL_CAN_MIN,ROLL_CAN_MAX,16);

        // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
        if (measure->yaw - measure->yawAngleLast > 180.0f)
        {
            measure->yawRoundCount--;
        }
        else if (measure->yaw - measure->yawAngleLast < -180.0f)
        {
            measure->yawRoundCount++;
        }
        measure->yawTotalAngle = 360.0f * measure->yawRoundCount + measure->yaw;
        measure->yawAngleLast = measure->yaw;

        break;
    case UPDATE_QUATERNION:
        measure->quaternion_ecd[0] = rxbuff[1]<<6| ((rxbuff[2]&0xF8)>>2);
        measure->quaternion_ecd[1] = (rxbuff[2]&0x03)<<12|(rxbuff[3]<<4)|((rxbuff[4]&0xF0)>>4);
        measure->quaternion_ecd[2] = (rxbuff[4]&0x0F)<<10|(rxbuff[5]<<2)|(rxbuff[6]&0xC0)>>6;
        measure->quaternion_ecd[3] = (rxbuff[6]&0x3F)<<8|rxbuff[7];
        measure->q[0] = uint_to_float(measure->quaternion_ecd[0],Quaternion_MIN,Quaternion_MAX,14);
        measure->q[1] = uint_to_float(measure->quaternion_ecd[1],Quaternion_MIN,Quaternion_MAX,14);
        measure->q[2] = uint_to_float(measure->quaternion_ecd[2],Quaternion_MIN,Quaternion_MAX,14);
        measure->q[3] = uint_to_float(measure->quaternion_ecd[3],Quaternion_MIN,Quaternion_MAX,14);
    default:
        break;
    }

}

static void DMIMULostCallback(void *imu_ptr)
{
    DM_IMU_Instance *imu = (DM_IMU_Instance *)imu_ptr;
    uint16_t can_bus = imu->can_instance->can_handle == &hcan1 ? 1 : 2;
    LOGWARNING("[DM] IMU lost, can bus [%d] , id [%d]", can_bus, imu->can_instance->tx_id);
}

DM_IMU_Instance *DMIMUInit(DM_IMU_Init_Config_s *config)
{
    DM_IMU_Instance *instance = (DM_IMU_Instance *)malloc(sizeof(DM_IMU_Instance));
    memset(instance, 0, sizeof(DM_IMU_Instance));

    // 注册到CAN总线
    config->can_init_config.can_module_callback = DecodeDMIMU;
    config->can_init_config.id = instance;
    instance->can_instance = CANRegister(&config->can_init_config);

    // 注册守护线程
    Daemon_Init_Config_s daemon_config = {
        .callback = DMIMULostCallback,
        .owner_id = instance,
        .reload_count = 20, // 200ms未收到数据则丢失
    };
    instance->daemon = DaemonRegister(&daemon_config);

    imu_instance[dm_imu_idx++] = instance;
    return instance;
}

void DMIMURequestData(DM_IMU_Instance *instance,DM_IMU_Update_Type_e update_type)
{
    uint8_t reg;
    switch (update_type)
    {
    case UPDATE_ACC:
        reg= 1;
        break;
    case UPDATE_GYRO:
        reg = 2;
        break;
    case UPDATE_ANGLE:
        reg = 3;
        break;
    case UPDATE_QUATERNION:
        reg = 4;
        break;
    default:
        return;
    }
    instance->can_instance->txconf.Identifier = 0x6FF;
    instance->can_instance->tx_buff[0] = (uint8_t)instance->can_instance->tx_id;
    instance->can_instance->tx_buff[1] = (uint8_t)(instance->can_instance->tx_id >> 8);
    instance->can_instance->tx_buff[2] = reg;
    instance->can_instance->tx_buff[3] = 0xCC;

    CANSetDLC(instance->can_instance, 4);

    CANTransmit(instance->can_instance,1);
}


/**
************************************************************************
* @brief: float_to_uint: 浮点数转换为无符号整数函数
* @param[in]: x_float: 待转换的浮点数
* @param[in]: x_min: 范围最小值
* @param[in]: x_max: 范围最大值
* @param[in]: bits: 目标无符号整数的位数
* @retval: 无符号整数结果
* @details: 将给定的浮点数 x 在指定范围 [x_min, x_max] 内进行线性映射，映射结果为一个
指定位数的无符号整数
************************************************************************
**/
static int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
    /* Converts a float to an unsigned int, given range and number of bits */
    float span = x_max - x_min;
    float offset = x_min;
    return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
}
/**
************************************************************************
* @brief: uint_to_float: 无符号整数转换为浮点数函数
* @param[in]: x_int: 待转换的无符号整数
* @param[in]: x_min: 范围最小值
* @param[in]: x_max: 范围最大值
* @param[in]: bits: 无符号整数的位数
* @retval: 浮点数结果
* @details: 将给定的无符号整数 x_int 在指定范围 [x_min, x_max] 内进行线性映射，映射结
果为一个浮点数
************************************************************************
**/
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
    /* converts unsigned int to float, given range and number of bits */
    float span = x_max - x_min;
    float offset = x_min;
    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}